import numpy as np
from calibration_utils import tcp_cali



points = [
         [312.97,-127.67,-116.68,1.65,39.17,187.07],
        [318.69,-129.73,-130.53,23.72,31.06,187.06],
        [303.96,-124.05,-111.19,-7.57,53.16,187.06],
        [275.14,-120.23,-114.45,2.04,102.37,187.07],
        [261.87,-129.41,-112.99,10.63,131.26,187.07],
        [308.47,-127.20,-123.43,15.71,38.45,187.06]

]
'''
find solution:  [[-0.00624954]
 [-0.22429878]
 [ 0.37709918]]
current best average dist:  1.5569158712564e-16
current max dist:  2.747498596334823e-16
current min dist:  5.494997192669647e-17

'''

points =[
        [16.8904 , -89.0503 ,-164.0372, -146.8934,-111.9646 ,  56.3912],
        [-63.0075 ,  49.7009 ,-130.3027 ,  31.8350 , -48.2738 , -12.0621],
        [109.7163 , -48.8444,  101.6915   , 0.2825  , 69.6764 ,  89.6895],
        [ -100.2001 , -35.9106 ,  93.1936 , -50.4896 , 141.7753,  -60.1214],
        [97.9564 , -50.5699,  120.1946 , 166.9572 , -46.8543, -158.6920],
        [-33.0750 ,  32.9867 , -97.8006 ,  -2.6677 ,-117.9886,  -20.8514]
]

"""
find solution:  [[-0.00624934]
 [-0.22430031]
 [ 0.37710047]]
current best average dist:  3.238150488487198e-17
current max dist:  5.55111512312091e-17
current min dist:  0.0
 
 """

test_joint_angles_radians = [np.array(angles) * np.pi / 180 for angles in points] # 输出结果 test_joint_angles_radians
data = tcp_cali(test_joint_angles_radians) #input joint angles should be in radians
data.run()

